#include <ros/ros.h>
#include <vision/RF.h>

class TestInput {

protected:
  ros::NodeHandle n_;

  ros::Publisher values_pub_;
  std::vector< std::pair<int, int> > locs_;

  ros::Timer timer;

  // Instance parameters
  std::string rf_name;
  int H, W, h_space, w_space;

public:
  
  TestInput(ros::NodeHandle &n) :
    n_(n)
  {
    timer = n_.createTimer(ros::Duration(0.1), &TestInput::process, this);
    ROS_INFO("Timer made");


    n_.param<std::string>("input_rf_name", rf_name, "default_rf_name");

    n_.param<int>("H", H, 240);
    n_.param<int>("W", W, 320);
    n_.param<int>("h_space", h_space, 15);
    n_.param<int>("w_space", w_space, 10);
  
    for(int w = 10; w < W; w += h_space)
      for(int h = 80; h < 160; h += w_space)
	{
	  std::pair<int,int> p(w,h);
	  locs_.push_back(p);
	}
    
    values_pub_ = n_.advertise<vision::RF>(rf_name, 1);
  }

  ~TestInput()
  {

  }

  void process(const ros::TimerEvent& event)
  {

    ////////////////////////////
    // Value generation
    //
    vision::RF vals;
    int num_pts = locs_.size();

    for(int i = 0; i < num_pts; i++)
      {
	vision::RF_value v;
	v.x_disp = locs_[i].first;
	v.y_disp = locs_[i].second;
	v.index = i;

	v.val = 0.5;
	vals.rf.push_back(v);
      }

    // Publish RF values
    try
      {
	values_pub_.publish(vals);
      }
    catch(std::exception e)
      {
	ROS_ERROR("Error publishing values");
      }
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "test_inputs", ros::init_options::AnonymousName);
  ros::NodeHandle n;
  TestInput rf(n);
  ros::spin();
  return 0;
}
